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[If ^JI 1 ] 

[M^JI 2 ] 

3 <Xtu ? a vM H ^WT^^T^Hi-^M^Jl 1 ic f 5 « <?> & ft fli!l il ?J v£ . 
[& 1 1 



P = P 0 + J[ u^v = P 0 + h £ u<f£ , 0 < j < 0 < S = |- < 1 



CD 



cos jS - sin p 0 
sin/? cos/3 0 

0 oi. 

a = a a +a t S + a 2 S 2 
J8 =b 0 +b l S+b 2 S 2 



cos a 0 sin a 

0 1 0 
-sin a 0 cosa 



cos /? coscc 
sin /? cosa 
- sin a 



(2) 



a: 




■*■() 


fa 










.V 



(3) 
(4) 

(5) 

& ft -P ft. 3 iX7U ? o V >f H ft jf$ _h <50^<jo 15 B ^ 9 V ji x &V-?-<7>#]SfHft^^-to 

^hT-IJofcl^ST'ibt. S li M iX 7C<7> ft T ») . 3 ft * ft # t of ^ . 

i , j , k tt * ft f ft . x M . ytfo.JkVzWiJifan&iiL^tb'i-e&Zo 
u lij&P icfclt £ ft*ft<7>JtJj6^|Sl*^-t*fi^ * h ^ T*#> 9 . 5t(2)t ioT4i^ti 
E k ^^E ia U@fe7h U?* 1 ), ft -P ft x kfli^^ftS/^lHlKS.^ 
j 1***3 9 ^Ma <?)0i^ib LTv>5. gO#* 3 - (yaw) % tf -y =f- ( p i t c h) 

Hitu 7„ x^(2)lt. i lb ft fa 9 h . * -f j It * £> *) \z a fzrt 0 L . L fc> 

£ <o *> ic k 16 £ t> *) \z /9 fi it 0 f c t \z «fc o T . *ft -5 ^ hiuA'Ubti^zU^LT 

a o , a j . a 2 > b q . b j , b 2 ti $C o 
[If^Jl3] 

ft IS 77 ft <T> fcf -v ft is J: V 3 — ft <^> ft f ft I 
£>ft£ 3 JX7cft*Sl ( 3 '{Kit ? u V -i H ftiMl ^n) 
fl3#*H3iL. c <7) 3 }X 7E ft H ic J: -d TIM^IS 
[»^JI 4 ] 

If Jf$ 77 ft <7> tf -v ^ ft J: 3 — ft ^ ft -? ft to* 
«bftS 3 iX7&ft*& ( 3 JtXtu ?ny>(H ft*S ^n) 

[«^JI 5 ] 

=1 > tf — 9 ~& , 

}f^77 v *n is «k v 3 -ft <7>^-ft-Pft)b*ft*S:K * /clift,||*^fc^ — iX^T-Jf^ 

«bftS3)X7Cft*ft (3)X7C?nv^ Kftifcv^T") ^ffluTlM #l» * tz It n - ^ 



^ t l± ft Jjfc j| 2? 8t -5-*. 



im^m 6 ] 

h ti Z 3 #C7Uffi m ( 3 #C7U 9 n y >f K ft j& ) ^fflv^T. Ili M a a o <7> ff^ 1£ ff "t £ 

3 i: £!f#$C ttSlIifa^lt^S. 



[mm%] mmm 

[ 0 0 0 1 ] 

*^BHti. d#'; XfF^M. Siii, if^f^^^M ( n t' v hWt\^ 

[*J1H«] 
[ 0 0 0 2 ] 

^> Tra * ffl v> t % it w i" £ § o 

[ 0 0 0 3 ] 

^ > a ra . 3 ix^ x 7- v >f > ?gn * ^ ***n ^ ft t s «b *i tc is o 

i ZWmmt LTlt. 3iX^*^5 >( >«IWfc**n<bftTv>s fF:£itK 1 #M) . 

[ 0 0 0 4 ] 

Li)X, 3 tfC^X 7° ^ >f mW^rftMM-Zft fz%\>n t LT 

ftTi^ § &i^8C ^ ft^t^^M^iftfta t <^M#**X^T*> & , 

[f£Bfl<7>^] 

[f£Bflfc*ftm L =fc 7 fctiSS] 
[ 0 0 0 5 ] 

2 TXTUlcfcv^T x. "bftfc&^fcffl* fflW^vi i: LT. ? n V >f K ffl ffi v£ <b 

T * n v >f K ft*ft* 3 #C7UlcM L . S */&*iJ<5Dliraicffli**U3\ ft*ft<7>J| $ W » t L 
TH 2 ft § ? n V >f K ft^<7>£ff & <k 0 . mmm*- felZtt o 9 . *ftj|lCJt* C T^fL£ i± 
fcUt4±^*(li!llP%#Ik:||ST?#Sfc®btlS. £ fz . ftiI$^7>-?tLTu 

3 ^TClc^^-t § 3 t li$({ift!J$B& if <7)^ffk:4yv>T*MT*»S 3 i: *»JW^r$ft*. 3 ft 
*Tk:^ny>fl<ftJj**3iX7Ck:fl£^Lfc s t<?)i:LTl±x L i b<7) " 3D Discrete Clotho 
id Splines" (#!ff fF^CilK 3 #M) * £ h ft T 5 fc*. ^^^1?^ n y ttlt 3 
iXrck:& i !BLfc'b<?>ti:*n<bftTv>$:v>. 5^ <?> T* <?> fl£ ^ tt . &flt ic g it} T § £ j£t 

fft-f&T* 5 o 
[ 0 0 0 6 ] 

> 2}X7C?n>M H (ft <?> iff ^ T S § £ 1+ 31 S *8 <* ft * 3 '<X tu ^ n y >f K ft 

[ 0 0 0 7 ] 

[#ff I^TK 1 ] W>iSflT • feffll8*f , " fE^-ftC A D/C AMy X -r- A " A ?t 

. 1997 

[#ff fflcm 2 ] tfL^ffl, JRff#, Mffl7^#. IHOjff^, " ^ n y ^ H tc § gtftiffl 
W v£ " ( H * a # v V -¥£U 8 ^ 6 , p p 4 0 - 4 7 ) 

[^Nf!^TS^3] Li Guiqing, Li Xianmin. Li Hu a "3D Discrete Clothoid Splin 
es " , (CGI '01, pp321-324) 

[ 0 0 0 8 ] 



[ 0 0 0 9 ] 

at jrji i <7>mni$ . fi&ST? ft<?) x v *n & * u 3 -n <r> % n^nij^m^ £ tiffins 

* fcli v - ?^W«$H1U C <7) 3 ^TUEft^iC £ o TlS<7>5Iift£flJfflP-t £&MfliiJ 
[0010] 

P = P 0 + £uds = P 0 + ft £urfS, 0 < j < ft, 0<S = — < 1 (i) 





cos J3 


-sin jS 


0" 


cos a 


0 


since 








cos jS cosct 


u = E tp E Ja (i) = 


sin /3 


cos /3 


0 


0 
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0 




: 




sin cosa > 
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0 


1 


— sin a 


0 


cos a 








-sin a 



a =« 0 +a,5 + a z S' 2 (3) 
P =b 0 +b l S+b 1 S 7 (4) 
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- p« - * 


-V.> * 











It * ft -P ft x 3 iX 7U ? a v >f H Eft $ _h <^ &<7> it B ~* ? h ^ x ^V-f-^^SBM^^-to 

* h T*9J o fcffi* STifct, S liMiX7C<50«T*> 9 . 3 ti* Eftf*^$C t of 
i , j . k It * ft % ft . x W x y$l.^^z$|ii7}|S]^^te^?h^T-£>£o 
u It & P EC £r tt 5 Eft M <?> *t 7? IS] * ^ -? & ~* ? N ^ T- #> 9 . 5^ ( 2 ) EC «fc o T 4- x. h ft 
£ o E k ^VE)«U0evh ') ^ T #> »K ^ ft -fil s k fi * 9 tf> ft Jg |9 ^ V 
j i*t)^ftSa^@it^blTv^„ h0#£ 3 - (yaw) M * -y (p i t c h) 

@itU7o 5* (2) It. i mijfaco h 'i* . £ i* j f i 9 EC a E+ 0 L . 

£ ?> EC k to * t> *) EC /9 r± rt 0 f C t EC =k o T . ^ * h^uA'f^Jl^CctiLT 

v~> <g> „ a o . a j . a b o » b [ . b 2 ti St o 
[0011] 

If ^JM 3 <7>f&Bfl \t . }f jf$ 7? |S] <7) f -v ^ft «fc v 3 -ft cn % ft ft *?* ffi H Tz liffifcfcj* 
^ft^-tf^T*-^ <b tlS 3 )X7UEil*t (3^7C^nv^ H iicvn ) ^fflv^TlIfi» 

[0012] 

-y ^ft 4? J; UE -ft<T)^ft^fthmm^^rz\t^m^^WL<^~K^T*H-z-hftZ> 3 /7X7nft3 
[0013] 

-y ^ ft 4? «k U 3 - ft * ft f ti ft* Eft H * t Ei Eft ^ * ^ SC — ^ T' ^ <b ^ 3 7C ft 
^ ( 3)yX7L^ n'MF ttic07 ) *m^TT.m®l%f£fzlt'7- ^^f^^ttt^ 
fSt LTft^^ i±^> tzbbco-^xi H C.XEJ^<y>7'a ^ A Ec J; S If g*5H% Ifi^ L =i 

> tf a. - ^fg^HX o nr IS &teiM So 
[00 14] 



m^M 6 <n ¥tm it » Jt *ft^l«l<?> ^ -y ^ ^ 4? «k V 3 — ft <?> ^ tif ti 3b s ft*ft^ * /!: tt tt*SU* 
^^^-zX^T-Jf x «b tl£ 3 )X7Uffi*l (3iX7C?ny^FftitU7) ^fflv^T. I US!! 

[0015] 

*^BHtcj:mi\ ft*ft<?)i^»3b»ftjjs^ * fc»ift«m^8c-c#> o . ^ commuted * v 

KlC W L Tilifro-e*) -5 Z t fc'ffSE $ tl -5 . f v^iiltt, -o^?D'M Hi^^ll 
[0016] 

[0017] 

WT*%HB tc o . 3 iX7C * n vM K ffl*ft<7>£3li:4f 3 }X7C ^ p y h' ftilc i 
£ffiWv£. 3 /Tvtu? a V >f F it W * ffl MOT T^vS ic T ABiXf&HU ~t Z . 
[0018] 

1 . 3 iX7C * n vM F ffilt^Sis i: ft 
(1) 3}X7C^n'M KioS*^ 

*nvMKft*ft(Clothoid curve) . SO £ 3 >i — ~ - <?> iSkM (C o r nu ' s s p i r a 1 ) £: «t of 
lift. ft*ft<?>^ 3 izitm LTft*^tfbt§ftiT'*^, 

t£**a «b tiT -5 2 /7v7c<y> ? n v ffi^ii . Xffitt&t ( 2 }X7cft*l) <jo — «t?*> ►) . 0 

2 iz ^ $ n % x y mm ±.\z & i> t . yk^ t- ^ 2 ti 3 . 

[& 3 ] 

P = P 0 + JV*<fc = P 0 +AjVV,S, 0<^</2, 0<5 = ^-<l (1) 
^ = c 0 +c,j: + c 2 f 2 =^ 0 +^ l ,5'+^5 2 (2) 

ZZT\ 

P = x+ Jy, (3) 

p 0 = x o + Jy 0 (4) 

e J * -cos(j> + jsin^ (5) 

It. ft 7? ft^ft^-t^fu^ f h 'I (M$frl<7>-<?h>l) T'£> 9 . *<7>7j|S] « 

T m y> "t -5 t E±tl ft X <?> j& P t)*& h tl £ o 

^) ^htt5. s * h T-SO o fctit* STibt, S l± iX 7C <*> ffi *> 0 . zti^ffi^ 
^ St i: nf ^ . 

* n V >f F (tt*£<7)Jff »l±. it ( 2 ) TX L «fc 7 lc. H 77 l«l ft * ft s * It Ett IS 

:ilb&y*ftl^)^Ih$?ny ^ K^^7 >-7 J:tf^ 0 Hf2l± — 

IMX^ 3 }7vX PC ffi^ L T . 3^7C?ny ^ KiI<?)ittfHo ^*3/7vX^^v 

3 iX7C ?o'MH iltWT^^TSHti. 



m 4 1 



P = P 0 + £u<fc = P 0 + h£udS, 0<s<h, 0<S = ~<1 



(6) 





cos /3 


-sin/3 


0" 


" cos a 
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sin a " 
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cos cos a 


u = E k{t E ia (i) = 


sin jS 


cos/J 
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<0 




sin p cQ$a > 
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— since 
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cos a 


0 




— sin a 



* ' V. > 



a = a Q 4-0,5" + a 2 S' 
J3 = b 0 +b,S+b 1 S 3 





X 




*0 


p = < 




>, p 0 =- 






i\ 







(7) 



(8) 
(9) 



(10) 



J 



It-Zn-Zii. 3 iX 7U ? n 7 -i K _h <?"> * <?"> a ~* ? H » V * <r> f] Kfl M * ^ -t . i 
k ^ tl f tl . x Ab . y «k ^.^zfi^fSj^O^-fi^ ? h iT*i)S. 

4 o j$ ( 7 ) lc is u T . E k 0 & V E i « tt 0 $e v h 'J ? XT*J> 1 s H 3 tc 5jk $ tl -5 =k 7 tc. 
* ft <? *u k $1 ( z $1 ) * t> *) <?> ft Jg P V j $1 ( y fi ) t^^<^ftS«<7)0$E^ 

ibLTi^. bu%£ 3 - (yaw) tttf-yf (pitcb)0fei:U7o ^ ( 7 ) li . i fib 

(xltt)7jfr<7)3ift^?h4.^ s U -f j $i ( y $i ) £ t> *) \z a r± rt m L . L -5 \z k 
M ( z fi ) * t> *J \z (9 fi 1+ 0 -t Z t \z Jc o T . }iH ^ ? V >i u t)*m h ft 3 c i: % ^ L T 

-ft M^i^:tl)'T'§5. tf -y ft a fc* 0 £ b . x yfffiT*tv^/:2iX7C^7n* 

t§M7? ft 7? ft -< f h ^ u tcii^/Jx^^ d s **MtTfft^"t* 3 /tvtc ? n V >f K 
tl -5. „ 

3 iX7C * u vM K ft M lc 4? T It . mU-< ^ h >v <n \£ v * ft a Jk U 3 - ft /9 & ^ tl f tl 
5$ ( 8 ) & ( 9 ) ic ^ i- «k 7 k: x ft !^ 4% ^ S S <^ 2 iX^T* H- x. h tl § . z^ztlcioT 

tc * £ . 

l-U^^lcioT^Lfcri: < . 3 iX 7U ? a V -f KftilJ r B ffli ~h 0 <r> tr -y ^ ft & V 3 
-ft**. * ti^tiMi«f(^-yx^Tft.b $ n« ft J tSi^ti^o 

P o ^ «b *n % Z> — o <^ 3 YK 7t 9 a y >f H -b r > > h ti . 
[»5] 



a 0' ^0' ^ 



(ID 



^)7i^)A-7 >-?lc ioT-3j£$ti§o a L b 2 ^ 6 o<?)f tttftg^f fit If ^ 

[0019] 

( 2 ) ©J^^i 

^ ( 7 ) icfcv^T . S*m*l7^f«I^ * h ^ i <?>n ») lc**M^3f, [ i , j, k]^ftAt§ 
i: . iX <n M W M (moving I r a m e ) E ^ f# -5 <, 



m 6 1 



E = [u v w] = E* E Ja [i j k] - E ip E Ja [i] = E* E Ja 





cos/3 cosa 


-sin /3 


cos/? sin a 






sin jS cosa 


cos/3 


sin j8 sin a 






-since 
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cosa 






cos j8 cosa 




— sin/T 




cos j5 sin a 1 


u = 


< sin jS cosa 




cos/3 


>, w 


- 


sin /3 sin a 




- sin a 




0 




cosa J 



(12) 



(13) 



j$ \Z m (ft 4 ffl IC # * tl £ iji. & ~* h ^T*cB 1 . 
[K7] 



tcift^ 
( F ^ 

F IC J: 



£ 0 = E^E'"" 



ft 
[ 

( 

a — 

T#+ 



(14) 
(15) 



<5 o 

0 0 2 0 ] 

3 ) n - y 7 

illt^St§: ticioT, 3 T^-^ (toll) J 

^Jf^ft^fc*. 3 ? n V >f F \zmW- £ *l£ ©J^lilc £ 0 ft *) 

tcii l fciifi a . #f^<7> u »? t a a EKt £ 3 t ib'T- § £ ^</>c 

& <7) ^ £ rt T tt ft 



IC ,k O 



E = E kp E Ja E ir I = E kp E Ja E ir (16) 
[& 9 ] 

y = c 0 +c 1 5' + c 2 S 2 (17) 
[ 0 0 2 1 ] 

(4)3 z^tu ? n vM F ft *l <^*$ ffl ^ ft M 
( a ) 3 ^tc ?nvn* ft*ft<7>>£<i 

3 iX7Gft*ft<7>>£*ft~* ? F ;Mi. f F ^ u &m^~Z )X<7)xtT-m £ ti& z h jb*£n 



[11 0] 



u' 



(18) 



f-a 1 (S) cos p (S) sina (S) - J3 \S) sin jS (5) cosct (S)" 
u'(5) = ]-a'(5)sin/3(5)sinct(S)-jS'(5)cosj8(5)cosc((5) . 

[ -a'(S)coscc(S) J (19) 

|^S)| = Va'(S) 2 + /W) 2 cos 2 a(S) 

"t 3 ^tu f n V >f H ftJfft<7>^Jji^ * h >i It . S^fflv^-CTie^fl^T^ 5 ft -5 

[111] 

u'GS") 



n(5) = 



Va'(S) 2 +/?'GS) 2 cos^aC?) 



(20) 



-a , (5)cos/3(5)sina(5)™j8 , (5')smj3(5)cosa(5')' 
-a * (5 )sin J3 (5) sin a (5) + 0 '(>!> ) cos jS (5) cosa(S) 
-a'CS'JcosaCS') 
[ 0 0 2 2 ] 

(b ) Si^ffluf; 3 iX7C 9 a v >f K ft&t<?)>£!S 

(7) <7>Jf*£u *>i*^ |B]*IHcY£*£n ic o i^T % t£T A & o nafltiM^ft ( 
1,0,0) icWLTs 1W»!^*ft^lSl*^SCr *fflv^T ( 0 , c o s v , - s i n v ) T'ibt 
d ft* J#*8 i: Is] £ «k 7 ic 0 |g £ -t± £ i: . v£*g n It T ie<?> <fc 7 $ ft § . 
[K 1 2] 



(21) 



cosjS(5) -sinjS(S) OT cosce(S) 0 sin a (£)"][' 0 
n(S) = sinj3(5") cosj3(5) 0 0 1 0 J cosv > 
0 0 1 -sina(5) 0 cosa(S') — sinv 

— sin v cos /3 (S) sin ct (S) — cos v sin ft (S) 
- -j ~ sin v sin /i (5) sin a(S) + cos v cos fi(S) 
-sin v cos a (S) 

(20) ( 21) <JO^^:ltfSi-^ £ . sin y , c o s y t± Tifik: jtfjft L T v> S 3 t frt> fc* -6 
[K 1 3] 



sm v = 



a'(S) 



COS "if = 



^a'(S) 2 + P'(S) 2 cos 2 a(S) 

p\S)cosa(S) 
^a'(S) 2 +p\S) 2 cos 2 a(S) 



(22) 



[ 0 0 2 3 ] 

( c ) 3 )xit ? n v >r h nmiz tsi-t z, mm^x'commMm 

3 )Xtz ? a V >f H 1i Mk: felt <S Jfl^ ttl 511^* 5 k: (22) «fc 9 



[114] 



tan v = (23) 

P\S)cosa(S) 

il^cT* £> tl \t £ C t ft* £> o 
[ 0 0 2 4 ] 

( d ) 3iX7Cf n'M Kfti^ft* 
3 Kit ?n'MH ftlS^ft^li . TK<7)^T*H£ *14 o 
[115] 

K (,y) = i! !l = J! !L = J! 1 (24) 



Ja ,2 +B a cos 2 a 

k(S)=1- E- (25) 

n 

i: ft £ tl £ » 
[ 0 0 2 5 ] 

(5)3 /Tvtu ? n vM F ft*t<^# g£ 
( a ) ftH^jMtt 

-ow^ny -f F -b r > > h ( Is] — cr> => > - ? T' ft t> 5 tl § ? n V >f F ) lzi5\^~Zl£ 

, *<7>miifrj3\ai<7> tr -v & -ft <)*f nijsitas<?) 2 £ ^ns^-e 

S ic n L T £> & C i: *»f*SE $ tl £ . a tl li . -o^)?n v^h't r>> M 

^Tli&i&^ft&arft^fc'iiijrcT&S o Lti'oT, v£«b ^T*'l4H<^^v^ft*|^#'b tl £ 

ft* ^iIMlc * & «k 7 ic ^ ^ > - 9 tIJRt§ zticioT, vf <b ^ 4 ^ t o £ §r<7> ft *fc 
tf^53l:i'-?l4. 3 tl^ ? a V >f FX7'7 
[ 0 0 2 6 ] 

( b ) m m 

* * ^ ft tc t> -tt *z 3 YK it ft H * ft tc ft § d i: § s i^u^^rfflllcfflv^c fc* 
[ 0 0 2 7 ] 

(c )mw\&mt#>m&'& 

0.B • P3 9ft ♦ Ja C ft m ft if<50^f5T ft *§ tt x ^a'MK^*7i-?^u<o^^0lcL, 
*><6 VM± x < n fr<r> => > - 9 ^lcJNf^<J0 WBtW -6 CtlCioTff^Zh 

S -5 o Z tl «b <7> ft M 14 ? n v -f F ft m <D — ft T* 9 . ?nMF^7t-7ntffluT 
§o Lfc**o T. t£*<^N C <D J: 7 lc. • P3 9ft • S tfeftfc&^lc =k o TieiEi - 

£ 7 *■ - -v h & ^ x. T m *) Vk 7 <fc^ ti 4 < . [B]C "7 t - "7 7 1 ^ffll^TtlLf; 1 M 

* x a *fctt(5(?)Uf IC 0 tl< 3 fclCioT, 2 ^ a V >f F * ff Z> 3 

i: ib*T* % % CO T' x 3 tl is T 2 iX 7C ^ a V -f F lc o T i" T' lc # h tl T H M * ffl "t £ 

-f5rt)*>x Ilc^btiTv^2)X7C^n'; ^ Kt^fct, P3 9R ^ fi. ifft & ^ ^> M SU co ft H 
«t x a t |S 1 1^ t ^gf ^ 3 t T'f IT' § I. . d <^ Jc 7 ft <@ 8U ft *ft IC o V^T [B] — CO ^ 
it 3 ?nvn* ft*ft^*fflv^S Z i: ^T'§ 5<^)T\ If Sfi * ^*feft"? § 4. 

[ 0 0 2 8 ] 

(d) ail Lc7>^$ 



3h Z> VMi mffi® ft ft *) \Z < v» Z k )b*#V^ . 3 }X 7U ? n V K tc i? V* T li . <y * 

B£ 3fc <?> ffi W J: 7 fcitoTffg-t -S^^^ftv^ t ^ b , ft *ft <?> > * ^ 

^-ySCitfELT^ ( 7 ) , (2 0) JkV (2 6) ^ic^i" «k 7 iz . ft^fffcg^. ft*** 
«.m®icj|<to<b*i& 0 c^cMi. ^^-tsKft^JP^^tc § ;btoTlr#Jft!tf $tT-£>£, 
:w;u:j:oT, ^idBtci+SBf m^mm l . > t -ft £*>WiiK*flpjR-*s c t 

T li 3 ti ^ to & <r> \z ffi ffl ft tf g % ^ i" -5 fc* , 9 n V -f KT'tt-^lc-b^>> h CT lc M 
ft $ ¥ & <?> ft fc*BE *a T #> 5 £ to . ^7 -y ? - %co Mfe ft £ T =fr fU X *> £ . 

[ 0 0 2 9 ] 

(e jMMMfflto^ 0^t$ 

ft*6<7)±l!P»<>*^3 s ifcttiEMfbS tifc^S ST'i> 0 . ft*ft<7>>^jg^tt leW 
JcoT. ftnMMft ir-<^Mt^#'l4^fi.#-tc Jfx. 5 3 k ft'Ti . # a ft if ic m «b tiT § 

WLT^*>f)ti5^)-?s VEJfc<7>tiimft<7> £ iz&lfo k tc # x. ^> ti/cft^ ^-fiK-t S'&^fc* 
ftv^ 0 * /c. ft*<7>tfg)b*^^ft£:to. aiftBf 31 >frflnilJg , b <b *u WMWiSh 

\z m c tz mm * ft 7 c h*?^. 

[ 0 0 3 0 ] 

(6) ft ^^>-5?<7V|4H 

^Ifelc «k tit* 3 iX7U ? n V >f H ft*ft<7>&^ ^ > - 9 ^ft^lc^i?i-^WttWT<^ k is *) 
X3h & . § ^ ^ > - 9 £ 3 i: lc <fc o T HI 3 co «k 7 lc 3 )X7C ^ a v >f K ftjjft 

U 3 <^S^-<7>ftifi^ftifi^^»S ft* « <t> 4 <^ «k 7 tc ft o 

Tfc *) . 3 /7v7u ?n«^H mmitmmM^m. t ft* t ^ttfj-t-s 3 A7tmm*mM-t z z 

k of 16 *> § C i: ^fc)b^ 0 

[ 0 0 3 1 ] 

illi, 3 /Xtu ?nv>(H ft ^* "7 > - ? <t>'I4iH^ $L k dt> fz >b <r> X- $> 2> * 

[ 0 0 3 2 ] 

[^ 1 1 





J5 =^+6,5*2 


=( const ) 




fc 0= s 






K ~ { CO/LV/ ) 




K OC S 





[ 0 0 3 3 ] 

3 )Xit ? a V -f H ftiT'l^ ft *6<^> ^ 9 >-^SlcWi--5ft*^^ fb^ TPJ L ^ i" < . 

• a T- |9 <?5 2 iX ^> Jl ft It tl li . ft * It — ^ fft X £> Z> . 

• a (9 <7> 2 iX<0 3S***>tllix ft*tt^^^^<50l?gillcit#IJT'*>^o 

• (9 a 2 iX^Jl3b*ftlttlljr. ft$li-^ftT'f>§. 

• (9 a <7> 2 ft*ti^^^^<^g^|lctb^JT^§o 
z<y> J: 7 ftttHii. Mi^UPft ^icfr^tcftlffl $ 



[ 0 0 3 4 ] 

( 1 ) m%fr%mffi<7>®L¥®&ft 

tc Jc -5 X m <0 MMM W ~£ ± * B & LI, 3 }X tu 9 n V >f K ft U ( 3 #C7U 9 n V >f H -b r 
> > H ) ^*$C**fifro L . 3 Kit ?nV>fK-fer>>hlcJ:o TlM^H 

% $'J fiP -t -5 . 

2 3 /Xtu ?pv^h mm^^<7>^^m^^izmu^ n-eo z tit. smi, 

. z^^kftit, MT<o J: 7 iciej* 3 ti -5 . 11^) 3 iX <?> 2 *§ 

[»16] 

Px,Q.) = Px M {0) 
Py,a) = Py M (0) 
Pz i (l) = Pz M (0) 
a / (l) = a I+I (0) (26) 

tanv J .(l) = tanv )+ »(0) 

JC f (l) = KT f+1 (0) 

3*Ui. ilc^T'l»^^h^i:a,i^^h^, ft*i:a. P iiMT- £> & <?> + 
[»17] 

Px i (l) = J Px f+! (0) 
Pz f (l)=Pz y+1 (0) 

costce,{I)-« (+1 (0)] = l (27) 
cos[j3,.(l)-/3 f+l (0)] = l 
tan v,. (1) = tan v 1+ , (0) 
JC,(1) = k:, +j (0) 

zz~e£ib>lz., 

cos[a i (l)-a f+l (0)] = l 

tan v,(l) = tan v, +1 (0) 

tan v,(l) = tan v 1+ i (0) 

«'/(!) _ «'m(0) 
jS' f (l)cosa f (l) jj' w (0)cosa w (0) 

va\(l)j3' /+l (0) = a' i+I (O)JS', (1) 

*S m Tie^* # * it li B & -5 - £ tt5 * £ z t *>» t> ^ £ . 



[818] 



Px t (l) = Px M QQ) 

Py i (D = Py i+l (0) 
p Zi (i) = Pz M {Q) 

cos[a f (l)-a f+I (0)] = l (28) 
cos[j3 ( .(l)-jS I+1 (0)] = l 
a f (l)j5, +1 (0)=a M (0)/3,(l) 
K t Q) = K M (0) 

[ 0 0 3 5 ] 

( 2 ) Mtt&&ft-S^HIK 

coffi&T*. (28)j**iSfc-* «fc 7 lc)X<7> 3 tfC7C * n y -i \t ftjjfctfwc v > - 7 3 
Jc 7 lc L T . }X * i: Vt «b ^ lc Jfj^i" S 3 iX7C * n vM K £E£ i*T' § 3 . 

[ 0 0 3 6 ] 

(b ftfcj&8¥**ft*ft<?>Jii^;&i: * * «k 7 \z . 3 #:tc ?n«MH 

il (^vo*tr>yh) tiwts. c^^^-ictt. (28)j**Sffc"t «k 7 ICffi^ 
^ > - 9 IfS^Wtt (a) «k 9 9 . t^ILiR^ltli:^^. c<7>f-fH 

^ik^x fi ^^f^^^aticftn^ ^ > - ? %ast5 . t\*-7 z tfrb. mm 1 0 

* *n ^ 7 > — ? : ft ^ ^ > — y 

: (28) *>5vm±**>-SB 

tlTv^if^-lc li, »WCfflV55 3iX7C?n>;>( K ft *6 tf> $C * # 9J Ic J: 0 it ^ L 

o^step tc^ttT. (Kixuff-t*. iistepj: l x nmmm t mmu ^ ^ 

h =k 7 icffiP^ Lfc^T?. 12 STEP i: L T l»I * -SC"t S * 7 tffiM^ffv^ 13 

STE P TtMt-St-fS =k 7 tcffiP^-t^o £<7>^>£<7>ff£*itf>«^*EI 5 tig-*. 

( 3 ) M#a5^tfS^J 



3D Discrete Clothoid Sp 1 i n e s co ^% Q <r> ^-^tf^X £ if W^^^J ^ \z TI^^: 2 o & 
ZZXU Discrete Clothoid Sp 1 i n e s iz o X Mm ~t £ . * i* . fflF^M-^^^JO ^TI^ 

fcti^ft^ptf^'), p <?> &T%)&m \z m c & r m o*o m tz tzm^n a l . pcQt^ 

£ J; 7 & #ft ff^ Q ^ fF & o CCTP^H*^nI*§tt^t, * V =f > Q li m C X £ 
H^Tr nl, iuTi^^&T'r ( n - 1 ) + 1 M co TR^^ f# o c i: \z & Z . iii^^ 7* 

tCfcv^T . t LTtSi^ ? h ^ b ^ ^^^^ LTft* fc i^^nU 

^5£#>£<, Tie<7)^ (29) ^mtz L . (3 0)<7>W»fc«/hfl:-*S# =r>Qfc 3 

D Discrete Clothoid S p 1 i n e s i: H 7 „ 
[« 1 9 ] 

(q.eP) (29) 

g||A a /: ir+1 | 2 , i = {0...n-l}, A 2 *, -2*, + (30) 

3 }X 7C ? a v -f K ffi H6 <?> db co m M ft It . r = 3 co 3 D Discrete Clothoid Splines^)* 
=f> Q^fF 9 . * c ^btlT"i*^t§. 111:^7 > - 9 aO. bOlco^T^ (31)T*$ 

[St 2 0 ] 




(31) 



3 co }f ifi -ft ft ^ * H >u t l± # •) =f > Q % ^ t § Ic f X ic fffi *n i: £ 0 T *> 9 . 3 <?> t 
i: 5^ ( 7 ) £: lc J: *) & ft a 0 , b 0 * S . d <J0 i: § * 'J =r > P <7) TK ^ T* & u * 'J zf y Q ic 

3D Discrete Clothoid Sp 1 i n e s <7> ^ift <?> ^ (2 9) t 3)^7U ^ n V >f K ft Jjft<^ a ^ 
it(8)t^^1±T^)S-t§tTie<7)jt(32)^^ , )u:o 0 
[»2 1] 



a0 3i +-al 3i + - a2 3i =a0 3i+! 



a0 3! +-al 3j + 



(32) 



3 J 



I a2 3i - a0 3(i+]) _, 



., a2^)|]SB(|i:t^. IBJtf > - y b 1 . b 2<of] S^fi t »£T § 

hit. 3 /Xtu f ^ V >f K ft^coft^<7)^J: >) Stti-t-So 3)X7U^ 
( 3 3 ) T« H $ tl ^. <7> X . 



[& 2 2 ] 



h 

r Wit fcjg ?g 3)(84)i::ft9 , h<D%] m ft J^&HSo 

^ V(al 3i + 2a2 3i ) 2 + (bl 3i + 2b2 3i ) 2 cos 2 (a0 3i + al 3i + a2 3f ) 
n 3j 

I 3Ci+l)| 

<7) ^ 7 > - 57 <7> f] $ ft £ - a - h > • 7 7'V>ftlC ioTtft^o ZfLlZ =k oTi^tlfc 
^ > - r7 3 }yv7U * n v >f K ftf«X/& L . ^5UM^ 3 iXtl ?o'MH ttiT'iKt 
* . fit Fig #0 * HI 6 & V 0 7 tc 5f -t o 
[ 0 0 3 7 ] 

3. 3#ctu?o v >f \tmm*m^tz%n$.mffljjxt 

[ 0 0 3 8 ] 

(1) 3 YKTt ? n V >f F ffiF^fflv^MftlJffl^ 

3 /TvTC ?o'MF ffiMft*ft%fflv*fc»fftf|NjfiP;frjSii. 0 8 tc 5p £ ft § iX<7>^)K ^ h tc £ 

o 

[ 0 0 3 9 ] 

(a )XMM©jfLi»<7>^tf (08. SI) 

hl^IMA'KtS, f^IM^fti^ (I1,>S, tool center point) tiXffii£liio§ 
iMiSraftlcfffifc>ftfciii^&#i»ft*ft (it^^#tf) <7>x*mm®iz&M-tz t z 

t XM&to&mit. mm (x. y. z) xmzn. is^^iii, i^iix 

tyft&tytilz & £ c t # < . iHfiCRi'otvis. 31 ift frj 8P <?> 3? 1 & PS tt . C60$fL»<7> 
[ 0 0 4 0 ] 

( b ) jgift ft M X It (138. S 2 ) 

®ii$.MWfrh<7>&&iz J; "9 . 3 /Xtc ^ n y FfflWtt*ltcv&oT. ft *ft ± *> ftj fiU & 
< Xm^<50iIS • ^nilS^v^S-tS *JiS*_L£IS>&fc*ir<7> =k 7 & iff f8! <?> Fi 

frxitmmx^MZ ■£* ft^oTv^^7>T'iiilT-fi)i5tfci^vnfi^b, fig* 

* n mff^ «j tc 4? u t it . m st it ii u mm \z um 2 ft t & & <r> v> ft m m -t & . 

[ 0 0 4 1 ] 

(c ) Sf^SO (08, S3) JkVii>\, f y7>li^l: J; § I M fi I • ^^^IfS ( F3 



8 . S 4 ) 

t b . ^ n^n^mmiztt^ zxm&^^&Tt^db % z t frx° b % * (c)^tt- 

Sli. mfatiSlz itliiT^ J: 7 icff ftibiiZ* M1±£jz fcv^T . ft a 'If 35 ^ It $g . 
ir^)i<)^^oTi^. J£>S $ ftfc^iftifSlc i£f£Bf F^^^-f ft tt\ Jjiftiff ffi 43 ^^ififtt! 

fc^&W. t^Mb^M-Z ftS, t Ufl, 3 ^TuTli ( x . y. z. A. m . 0 
) i: 55: -5„ fcCL ( A * m . y . 0 ) U^iE^f L ( A * m . y 

) V fffi 0 $5 ill ^ . 9 ti 0 ^ ft ^r^i" . 

isfc. SScfi^JfH^^W^ic ck 0 . 3 YKJt * n v >f h fflWft*ftfcv&o *fc ft ^ }g 
£ ^tv££l+ * 7 -b -y h L * 7 -b -y h T . 3 ti* ^7 >y ? - m 7. (IS4"l><7>fiS* 

) C<^|+m^. ft * o T V* S <7)T . #|T'J)5. 

[ 0 0 4 2 ] 

(d)31M» (0 8. S 5 ) 
Sli — J&lcitff&fltfl? ( inverse kinematics) t of & ft T u S . M x. \t 6 $i<?> n # >y h fc»£> 

stts^ wffj**6 mentis. Bfc^wtiK zhccomw. ^w^mwv^M 

S@iLt^TlM^^fig-f»^^. 3ft**KB*8l«»i:i^a:ft*. j^«l«»t±. 
cfttii/xj^tclia^^M^ftg-^il^'bifi^ra^lHlKft^ 1^0 6^^a6S^^T**> 

S o 

ft * <7> a # -y icout flU SO tcfStfflf Lite < . 
[ 0 0 4 3 ] 

(e Jttffi^^k: «k £ &iifct- ?^ftc7>ffS (08. S 6 ) 

Bf7>9J $ nfc&IMt-) §a£f»«8P*#«>* c ft^g-li^- 57 (jftSft-r * - 31- ? 

(ft mm) *m^T . ^^xtis^itt^titr-? t lt^^s. 

±15 x ( a ) Jkv ( b ) itmm^Stc^imx^ *) . -Jgfcltfr * t>ftS. (c)-(e) 

5 ft S o 

±IQ<7)^T<^g+S^ . ®C M $IJ P ^ rB <7> * T tf 7 C ^dT^T-^ 0 . ifcSvMi. ( a ) & 
V ( b ) % SU<7> 3i > ^ - 5 ( ) IC J: *) IfSJStWa^ LTJ5§, ^ <7> ttifft ^ ^ > - 
? * ^^^fftiOP^glcM ») iA^. ( c ) - ( e ) <7)3\-M-&$a$.MBnW.ft-?if 

[ 0 0 4 4 ] 

( 2 ) n c^ih c n c mm. 

WTx ^^^^ffiftiJP^rH ( N C HE!) £ i" S if£" t . 7' a r ^ v <y> £ Jf o 
3>ta-ykNCSakii*-#ft:$nfcCNC^g4j$fflt4i^(:-3^Tiftiflt4. 
[ 0 0 4 5 ] 

(a)8|wf^NC^iltfflv^S^ 

^*<^ii^<^ N C tlMT'ii . 7' a r => 5 > ^^tfoTNCT-^^fflttS^n 777 
i: x C <7) N C ^- 57 $ ^ uTiWIIiatil^tN CIIJ:^)-o^IIlC^- H "7 x T 7 i?* 
^i^tlTv^. ^ ft tc W L T x M]S<50 C N C iWiT- 1± x ^n^7 5>rtft73>fa 



£ ~? . BU % <50 . ^aZI^ON C^Itffll^falCOl^T, 3 )X 7U * a 7 >f K IC J: § » fl fell 

8P^^*ffilg-r S. C^Jt ^-. ? n y ^ k t-- ? tti L C li ? p V K a- 7 > - ? $ 
ffloit^tU G =i - H <r> ^ ic ? n y -f K <r> ~7 * - v -y h £ Sit -5 . Z titt $J z. It . 

G *fc *fc *fc 

AO, Al, A2, BO, Bl. B2. H 

G***ttG3-H^)|ft*t. AO — Hti 3 /Xtu ? a V -f F -fe r > > h <r> 7 
-o<?>'*<7 > - 7 t^t. Z^3-K*Htft5ilc, ISllP()^fiiaic*Tv^, NC 

c^^fp^ riw»J tui. ik»*n cnuffl-z?f im&iz^- vco^mit-zm <• 

@ ft jb*<b ?#>S ft*. * #>IC N C IgT'fl . *> § ft <r> 'MM * <fc^ ^ -5 . Gn-FT^ 

m t & * c 

[ 0 0 4 6 ] 

(b ) C N C ^ 

7' a r ^ v <7> * o 3 > - - ? fcNCt§Hi:<7>--frft:£ttfcCNCi§g<7>fl§£- 

? a V A Y \Z m~t £ If ^<^g^<^ — H T?tf *l £ 
SSti^'ivi. £ tz . t' - ? <r> m $z M <r> * f - K «t W i* § ti o o § . 
— I£ ic . c^7*n vicii, * fi f ti </> ^ [c iS L ? a v K <r> >i=j > - 7 % v£ S 

* 3® * fl £ . 3 tl * r il » J ff IC tt s fjiif, U < O fc* <7> HlfC & 

) *fc # * tl -5 3 t > o 
[ 0 0 4 7 ] 

c 3 ) 3 /^tc ? o v ^ h* ffim*fflv^»faftijap^^<7)j|f m 

[ 0 0 4 8 ] 

Jlll^ifti'^ > - 9 IC J: >)il$nTv^X^7 > ft * if <7) ftfe<50 ft *| T" . ^Sft 
3 ti m -f £ ic . 0 9 tc ^ $ ti -5 «fc 7 tc . 7x 7° 7 -f > ft H R ( t ) T- ^ $ ti § |fi» _h 

^^iffit^ti, *^i^MJSii^<?>XA^^»r« a s i±*3^s n^i^w t itmm t 

fc>ft*|j% *MC If t <?>Tli^v^T-. 3*jA^$C<7>^fLS A tttll^lcii^ftiCfc 

li?S4v\ R 0 +AS = R(t 0 +At)<7)jt^^v^TAt^?t<a6^ti-tia\ i m£L~%M 
tilts 3 fc &o<7>T. -S^WraPS^lc Zc^ffS^^ »J $rt**Uf «: «b 3 

tC * ^ o 

[ 0 0 4 9 ] 

z t frM'&z fi. znizttm-tz&mmwmmit. mmmm^m^^^> . ti^mizm 

m<r>'J?*\.*MWfemt z t frWWZ tis. >f *r-?i±. ft^^fb 

[ 0 0 5 0 ] 

(c ) 3 iX7C^ n V >f H ft*ftl±x *tf>Jf#5fc&Jf£-i: LT, P3 # . ISi£tt*6«:^*# 

A,-? 4? ►) . MSO^ft^^^ffl^aAt? :U<, ^ ftllic ttt * 8c«fMfflH»S5*jKiBk: 

[ 0 0 5 1 ] 



(d) 3 Alt ? n v >f H ftHliM^te^ t *) ?j\z «k <b S^^jg^T*>*. x, y, 
z Ah T* ft *§ 3 *i 3 <?> N C m H li . 00 x ^ - ? % ic ffl it T fln X -f 2> £ § fc 
. n - ? <?) SX 9 ft It 7j \Z «k o T li . Ift^I^WILtt^of: 9 . ftn X L IC < ■? fz*) 
-? Z> z\ £ 3b Z> „ 3 iX7C ^ o v >f H ftJjSTli. fti^illc ioT4ibii^^T\ f4a6 

[ 0 0 5 2 ] 

*%BH<7> 3 /Xtu ?p 1 MF ft^fUc intt, X^Ma^^tfX^lc^W i: £ £ S W ft *§ </> 

[0ffi<7>fS^ 5 5ri&HH] 
[ 0 0 5 3 ] 

[01] x. y&lg±<?>2iX7C*nvMh-ftjjfcfc^i-|g 0 

[02] 2/X7C^n7O-ffiit^t0o 

[0 3] 3 #C7G * n vM K ft*ft<?> a . ^^it^tH. 

[0 4] ttilttfcft$i:<7)i1l$^tr7 7„ 

[0 5] If^t^tlSS. 

[0 6] 3)X7C?n'M Hffifflfti^fJ (ft*§UcSft&*£&. J^Fffft&ft*¥g£>rt 
[0 7 ] 3 z^tu ?nv^F fflWft*l^46^^'b^ft*l^ t ft*<o L i 

o 

[0 8] Sfi©Ji]^vS^^-tX^0o 

[09] x 7- v >f y mm~%^-?\£$%mo 



[mm%] mm 
im 1 ] 






0 0.5 1 



[0 5 ] 



! STEPl 


i 






STEP2 




-i • asEit is 




3 a y 
















■ im^m 

















STEP3 



[0 6 ] 




[ms ] 









P = P (s) , E 








s = s (t) 


Si) ft 




- (S2) 





- (S3) 






1 



mm 


- (S5) 


V 







- (S4) 



(S6) 



t=n -At 

P = P(t) , E = E(t) 

0 i =0 i (Pj , Ej ) 
6 i =0 i (t) 



[m 9 ] 





mm 
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